#pragma bank 255

#include "math.h"

const uint8_t atan2_table[20][18] = {
    { 64, 64, 64, 64, 64, 64, 64, 64, 64, 64, 64, 64, 64, 64, 64, 64, 64, 64 },
    {  0, 32, 45, 51, 54, 56, 57, 58, 59, 59, 60, 60, 61, 61, 61, 61, 61, 62 },
    {  0, 19, 32, 40, 45, 48, 51, 53, 54, 55, 56, 57, 57, 58, 58, 59, 59, 59 },
    {  0, 13, 24, 32, 38, 42, 45, 48, 49, 51, 52, 53, 54, 55, 55, 56, 56, 57 },
    {  0, 10, 19, 26, 32, 37, 40, 43, 45, 47, 48, 50, 51, 52, 53, 53, 54, 55 },
    {  0,  8, 16, 22, 27, 32, 36, 39, 41, 43, 45, 47, 48, 49, 50, 51, 52, 52 },
    {  0,  7, 13, 19, 24, 28, 32, 35, 38, 40, 42, 44, 45, 46, 48, 48, 49, 50 },
    {  0,  6, 11, 16, 21, 25, 29, 32, 35, 37, 39, 41, 42, 44, 45, 46, 47, 48 },
    {  0,  5, 10, 15, 19, 23, 26, 29, 32, 34, 37, 38, 40, 42, 43, 44, 45, 46 },
    {  0,  5,  9, 13, 17, 21, 24, 27, 30, 32, 34, 36, 38, 39, 41, 42, 43, 44 },
    {  0,  4,  8, 12, 16, 19, 22, 25, 27, 30, 32, 34, 36, 37, 39, 40, 41, 42 },
    {  0,  4,  7, 11, 14, 17, 20, 23, 26, 28, 30, 32, 34, 35, 37, 38, 39, 41 },
    {  0,  3,  7, 10, 13, 16, 19, 22, 24, 26, 28, 30, 32, 34, 35, 37, 38, 39 },
    {  0,  3,  6,  9, 12, 15, 18, 20, 22, 25, 27, 29, 30, 32, 34, 35, 36, 37 },
    {  0,  3,  6,  9, 11, 14, 16, 19, 21, 23, 25, 27, 29, 30, 32, 33, 35, 36 },
    {  0,  3,  5,  8, 11, 13, 16, 18, 20, 22, 24, 26, 27, 29, 31, 32, 33, 35 },
    {  0,  3,  5,  8, 10, 12, 15, 17, 19, 21, 23, 25, 26, 28, 29, 31, 32, 33 },
    {  0,  2,  5,  7,  9, 12, 14, 16, 18, 20, 22, 23, 25, 27, 28, 29, 31, 32 },
    {  0,  2,  5,  7,  9, 11, 13, 15, 17, 19, 21, 22, 24, 25, 27, 28, 30, 31 },
    {  0,  2,  4,  6,  8, 10, 12, 14, 16, 18, 20, 21, 23, 24, 26, 27, 29, 30 }
};

uint8_t atan2(int16_t y, int16_t x) BANKED {
    x = CLAMP(x, -19, 19);
    y = CLAMP(y, -17, 17);

    if (x >= 0 && y <= 0) {
        return 64 - atan2_table[x][-y];
    } else if (x >= 0 && y >= 0) {
        return 64 + atan2_table[x][y];
    } else if (x <= 0 && y >= 0) {
        return 192 - atan2_table[-x][y];
    } else {
        return 192 + atan2_table[-x][-y];
    }
}
